/*
 *  car.cpp
 *  
 *
 *  Created by Brian Bilbo on 12/03/09.
 *
 */


#include "car.h"
#include "node.h"
#include "map.h"
#include <vector>
#include <iostream>

bool Car::withinRadius(Position *pos1, Position *pos2, int radius) {
	return false;
}

void Car::driveTo(Position *pos) {
	Node* startNode = nearestNode(myPosition->posX, myPosition->posY);
	Node* endNode = nearestNode(pos->posX, pos->posY);
	std::vector<Node*> shortestPath = findShortestPath(startNode, endNode);
	vector<Node*>::iterator iterate;
	//cout << "\n\nshortestPath.size(): " << shortestPath.size() << "\n\n";
	
	cout << "\nDriving from " << myPosition->posX << ", " << myPosition->posY << " to (";
	cout << shortestPath[0]->row << ", " << shortestPath[0]->col << ") ";
	
	myPosition->posX = shortestPath[0]->col*10;
	myPosition->posY = shortestPath[0]->row*10;
	
	cout << myPosition->posX << ", " << myPosition->posY << "\n";
	
	return;
	
	Direction newDir;
	turn newTurn;
		Node* temp = shortestPath[0];
	Node* tempNext;
	while(!route.empty()){
		route.pop();
	}
	
	currSpeed = turnSpeed;		// Will prevent any problems with driving
	if(temp->row == myPosition->posY * 10){
		if(temp->row > myPosition->posY * 10){
			myDirection = West;
		}else{
			myDirection = East;
		}
	}else{
		if(temp->col > myPosition->posX * 10){
			myDirection = North;
		}else{
			myDirection = South;
		}
	}
	
	for(int ii = 0; ii < shortestPath.size() - 1; ii++){
		temp = shortestPath[ii];
		tempNext = shortestPath[ii+1];
		if(temp->row == tempNext->col){
			// Must be changing in X direction
			if(temp->row > tempNext->row){
				newDir = West;
			}else{
				newDir = East;
			}
		}else{
			// Must be changing in Y direction
			if(temp->col > tempNext->row){
				newDir = North;
			}
			else{
				newDir = South;
			}
		}
		// New Direction found
		newTurn = new turn(temp, &newDir);
		route.push(newTurn);
		// Insert position and direction into route
	}
	// Still need to insert last turn
	temp = shortestPath[shortestPath.size()-1];
	if(temp->row == pos->posY * 10){
		if(temp->row > pos->posY * 10){
			newDir = West;
		}else{
			newDir = East;
		}
	}else{
		if(temp->col > pos->posX * 10){
			newDir = North;
		}else{
			newDir = South;
		}
	}
	newTurn = new turn(temp, &newDir);
	route.push(newTurn);
}

void Car::driveTo(int xPosition, int yPosition) {
	Position* newPosition = new Position(xPosition, yPosition);
	driveTo(newPosition);
	delete newPosition;
}

void Car::gas() {
	currSpeed += accelRate;
	if (currSpeed > maxSpeed) {
		currSpeed = maxSpeed;
	}
}
void Car::brakes() {
	currSpeed -= accelRate;
	if (currSpeed < 0) {
		currSpeed = 0;
	}
}
int  Car::checkDistanceToTurn() {
	turn nextTurn = route.front();
	int tPosX = nextTurn.Intersection->col * 10;
	int tPosY = nextTurn.Intersection->row * 10;
	int dist = 0;
	switch(myDirection){
		case North:
			dist = tPosY - myPosition->posY;
			break;
		case South:
			dist = myPosition->posY - tPosY;
			break;
		case East:
			dist = tPosX - myPosition->posY;
			break;
		case West:
			dist = myPosition->posY - tPosX;
			break;
	}
	return dist;
}
void Car::move() {
	switch (myDirection) {
		case North:
			myPosition->posY + currSpeed;
			break;
		case East:
			myPosition->posX + currSpeed;
			break;
		case South:
			myPosition->posY - currSpeed;
			break;
		case West:
			myPosition->posX - currSpeed;
			break;
	}
}

Car::Car(Position *givenPosition, Position *dest) :
currSpeed(0), maxSpeed(10), turnSpeed(1), accelRate(1), myDirection(North) {
	destination = dest;
	myPosition = givenPosition;	
}
Car::Car() :
currSpeed(0), maxSpeed(10), turnSpeed(1), accelRate(1),myDirection(North) {
	destination = new Position();
	myPosition = new Position();
}

Car::~Car()
{
	
}


void Car::TakeTurn() {
	
	turn nextTurn;
	int dist;
	nextTurn = route.front();
	move();
	dist = checkDistanceToTurn();
	if (dist == 0) { // At turn
		myDirection = *nextTurn.Turn; // Make the turn
		route.pop(); // Remove turn from route
	} else { // Not at turn
		if (accelRate * dist <= currSpeed * currSpeed) { // Need to slow down
			brakes();
		} else {
			gas();
		}
	}
}

Node* Car::nearestNode(int xPosition, int yPosition) {
	int nearestX = xPosition / 10;
	int nearestY = yPosition / 10;
	return map->nodes[nearestY][nearestX];
}

std::vector<Node*> Car::findShortestPath(Node* startNode, Node* endNode) {
	std::vector<Node*> shortestPath;
	
	int myrow = startNode->row;
	int mycol = startNode->col;
	
	int destrow = endNode->row;
	int destcol = endNode->col;
	
	if (myrow != destrow) {
		if (myrow < destrow) {
			shortestPath.push_back(map->nodes[myrow + 1][mycol]);
		} else {
			shortestPath.push_back(map->nodes[myrow - 1][mycol]);
		}
	} else if (mycol != destcol) {
		if (mycol < destcol) {
			shortestPath.push_back(map->nodes[myrow][mycol + 1]);
		} else {
			shortestPath.push_back(map->nodes[myrow][mycol - 1]);
		}
	}
	
	/*int startNodeII = startNode->row*10 + startNode->col + 1;
	cout << "\nstartNodeII: " << startNodeII << "\n";
	int endNodeII = endNode->row*10 + endNode->col + 1;
	cout << "\nendNodeII: " << endNodeII << "\n";
	
	for (int ii = 0; ii < map->shortestPaths.size(); ii++) {
		if (map->shortestPaths[ii]->start == startNodeII
			&& map->shortestPaths[ii]->end == endNodeII) {
			cout << "\nmap->shortestPaths[ii]->path.size(): " << map->shortestPaths[ii]->path.size() << "\n";
			for (int jj = 0; jj < map->shortestPaths[ii]->path.size(); jj++) {
				cout << "\n\nmap->shortestPaths[ii]->path[jj]: " << map->shortestPaths[ii]->path[jj] << "\n\n";
				int nodeRow = map->shortestPaths[ii]->path[jj] / 10;
				//cout << "\nnodeRow: " << nodeRow << "\n";
				int nodeCol = map->shortestPaths[ii]->path[jj] % 10;
				//cout << "\nnodeCol: " << nodeCol << "\n";
				
				shortestPath.push_back(map->nodes[nodeRow][nodeCol]);
			}
		}
	}
	
	for (int ii = 0; ii < shortestPath.size(); ii++) {
		Node* shortestNode = shortestPath[ii];
		//cout << "\nNODE\n";
		//cout << "\nshortestNode->row: " << shortestNode->row << "\n";
		//cout << "\nshortestNode->col: " << shortestNode->col << "\n";
	}*/
	
	return shortestPath;
}